#include "./handler_task.h"
#include "../service/ota.h"
#include "../driver/cmd_uart.h"
#include "../driver/elock.h"
#include "../driver/ctrl.h"
#include "../driver/sensor.h"

namespace task {

HandlerTask handlerTask;

using namespace driver;
using namespace conn;
using namespace utils;

static void on_door_lock_ctrl(const Message &msg) {
    Error err = elock.open();
finish:
    cmdUart.res(msg, err);
}

static void on_motor_reset(const Message &msg) {
    uint8_t row, col;
    Error err = msg.parse_req(row, col);
    if (err) {
        goto finish;
    }
    err = ctrl.cargo_is_valid(row, col);
    if (err) {
        goto finish;
    }
    err = ctrl.run_with_state(row, col, false);
    if (err) {
        goto finish;
    }
    err = ctrl.run_with_state(row, col, true);
finish:
    cmdUart.res(msg, err);
}

static void on_motor_reset_row(const Message &msg) {
    uint8_t row;
    Error err = msg.parse_req(row);
    if (err) {
        goto finish;
    }
    if ((row < 1) || (row > 8)) {
        err = ec::INVALID;
        goto finish;
    }
    err = ctrl.run_with_state(row, false);
    if (err) {
        goto finish;
    }
    err = ctrl.run_with_state(row, true);
finish:
    cmdUart.res(msg, err);
}

static void on_pick(const Message &msg) {
    uint8_t row, col;
    Error err = msg.parse_req(row, col);
    if (err) {
        goto finish;
    }
    err = ctrl.cargo_is_valid(row, col);
    if (err) {
        goto finish;
    }

    err = ctrl.run_pick(row, col);

finish:
    cmdUart.res(msg, err);
}

void HandlerTask::run() {
    constexpr uint32_t POLL_TICK = 1000;

    Message msg;
    for (;;) {
        if (osOK == m_mq.poll(msg, POLL_TICK)) {
            dispatch(msg);
            continue;
        }
        ctrl.led_poll(POLL_TICK);
    }
}

void HandlerTask::dispatch(const Message &msg) {
    m_task_info.seq = msg.seq();
    m_task_info.cmd = msg.cmd();

    switch (m_task_info.cmd) {
        case proto::CMD_DOOR_LOCK: on_door_lock_ctrl(msg); break;
        case proto::CMD_MOTOR_RESET: on_motor_reset(msg); break;
        case proto::CMD_MOTOR_RESET_ROW: on_motor_reset_row(msg); break;
        case proto::CMD_PICK: on_pick(msg); break;
    }

    m_task_info.reset();
}




}
